#include "MarkerEnvironment.h"

namespace TUMAR { namespace CountryRoads {

	MarkerEnvironment::MarkerEnvironment(void)
	{
	}
	
	MarkerEnvironment::~MarkerEnvironment(void)
	{
	}

	const Marker* MarkerEnvironment::GetBoardPositionMarker() const
	{
		return lookup.count(this->BoardPositionMarkerId) > 0 ?  lookup.at(this->BoardPositionMarkerId) : NULL;
	}

	const Marker* MarkerEnvironment::GetBoardScaleMarker() const
	{
		return lookup.count(this->BoardScaleMarkerId) > 0 ? lookup.at(this->BoardScaleMarkerId) : NULL;
	}

	double MarkerEnvironment::GetBoardScale() const
	{
		return this->scale;
	}

	const std::vector<const Marker*>& MarkerEnvironment::GetOutOfBoardMarkers(void) const
	{
		return this->outOfBoardMarkers;
	}
	
	const std::vector<std::tuple<const Marker*, int, int>>& MarkerEnvironment::GetInBoardMarkers(void) const
	{
		return this->inBoardMarkers;
	}

	bool MarkerEnvironment::IsBoardVisible(void) const
	{
		return this->GetBoardPositionMarker() != NULL && this->GetBoardScaleMarker() != NULL;
	}

	bool MarkerEnvironment::CalcMarkerPositionInGameBoard(const Marker* m, int boardWidth, int boardHeight)
	{
		/**
		 * Basic idea: 
		 * - calculate vector from BoardPositionMarker to UserMarker 
		 * - rotate this vector into coordinate frame of BoardPositionMarker
		 * - take the vector as origin of a line and the z-axis from the BoardPositionMarker Pose (3. column) as direction
		 * - intersect this line with GameBoard plane (its normal is [0, 0, 1])
		 * - scale x and y component of the intersection point with the GameBoard scale 
		 */
		const Marker* p = this->GetBoardPositionMarker();
		
		double distanceX = m->Pose[3]  - p->Pose[3];
		double distanceY = m->Pose[7]  - p->Pose[7];
		double distanceZ = m->Pose[11] - p->Pose[11];

		double distanceX2 = (p->Pose[0] * distanceX + p->Pose[4] * distanceY + p->Pose[8] * distanceZ);
		double distanceY2 = (p->Pose[1] * distanceX + p->Pose[5] * distanceY + p->Pose[9] * distanceZ);
		double distanceZ2 = (p->Pose[2] * distanceX + p->Pose[6] * distanceY + p->Pose[10] * distanceZ);
		
		cv::Vec3f vector(distanceX2, distanceY2, distanceZ2);
		cv::Vec3f normal(p->Pose[8], p->Pose[9], p->Pose[10]);

		// plane normal [0, 0, 1] cancels x and y components, so only use z component
		double d = -distanceZ2 / normal[2];

		// save for visualization
		dx = d * normal[0] + distanceX2;
		dy = d * normal[1] + distanceY2;
		dz = d * normal[2] + distanceZ2;

		int tileX = (int) (dx      / this->GetBoardScale() + 0.5);
		int tileY = (int) (dy * -1 / this->GetBoardScale() + 0.5);
		
		bool inBoard = !(tileX < 0 || tileX >= boardWidth || tileY < 0 || tileY >= boardHeight);
		
		if(inBoard)
		{
			this->inBoardMarkers.push_back(std::tuple<const Marker*, int, int>(m, tileX, tileY));
		}

		return inBoard;
	}
	
	TileType MarkerEnvironment::GetTileType(const Marker* marker) const
	{
		if(marker->MarkerId == this->UserMarkerRoadStraightId)
			return RoadStraight;

		if(marker->MarkerId == this->UserMarkerRoadCurveId)
			return RoadCurve;

		return Empty;
	}

	void MarkerEnvironment::Update(const MarkerContainer& markers, int boardWidth, int boardHeight)
	{
		this->markers.assign(markers.begin(), markers.end());
		
		this->lookup.clear();
		this->inBoardMarkers.clear();
		this->outOfBoardMarkers.clear();

		for(int idx = 0; idx < this->markers.size(); idx++)
		{
			Marker* marker = &this->markers[idx];

			this->lookup[marker->MarkerId] = marker;
		}

		if(!this->IsBoardVisible())
			return;

		const Marker* positionMarker = this->GetBoardPositionMarker();
		const Marker* scaleMarker = this->GetBoardScaleMarker();

		double boardDiagonalLength = length(cv::Point2d(boardWidth, boardHeight));
		double distance = length(cv::Point2d(positionMarker->Pose[3] - scaleMarker->Pose[3], positionMarker->Pose[7] - scaleMarker->Pose[7]));
		
		this->scale = distance / boardDiagonalLength;
		
		// make this a for-loop, if we have more than one player
		if(lookup.count(this->UserMarkerRoadCurveId) > 0)
		{
			if(!CalcMarkerPositionInGameBoard(lookup[this->UserMarkerRoadCurveId], boardWidth, boardHeight))
			{
				this->outOfBoardMarkers.push_back(lookup[this->UserMarkerRoadCurveId]);
			}
		}

		// make this a for-loop, if we have more than one player
		if(lookup.count(this->UserMarkerRoadStraightId) > 0)
		{
			if(!CalcMarkerPositionInGameBoard(lookup[this->UserMarkerRoadStraightId], boardWidth, boardHeight))
			{
				this->outOfBoardMarkers.push_back(lookup[this->UserMarkerRoadStraightId]);
			}
		}
	}

}}